# Transport car 视觉部分

import sensor, image, time, struct, math
from pyb import UART, Timer, LED
from userservo import userServo
import right_angle


# 要找寻的目标的颜色
colors_threshold = {
    'orange_threshold' : (33, 66, 43, 74, 40, 72),
    'green_threshold' : (19, 64, -55, -9, 21, 54),
    'blue_threshold' : (19, 63, -15, 9, -47, -13),
    'black_threshold' : (0, 23, -17, 0, -11, 6)
}
# 小车的移动
car_move = {
    'stop': 0x00,
    'turn_left':0x01,
    'turn_right':0x02,
    'go_forward':0x03
}

uart3 = None
command = 0x01

def sensor_init():
    sensor.reset()
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QQVGA)
    sensor.skip_frames(time = 2000)
    return

def uart3_init():
    global uart3
    uart3 = UART(3)
    uart3.init(baudrate = 115200, bits = 8, parity = None, stop = 1, timeout_char = 1000)
    return

def send_data_packet(direction):
    '''
    @brief: 发送数据包
    '''
    global uart3
    temp = struct.pack("BBBBBB",
                    0x55,
                    0x55,
                    0x01,
                    0x00,
                    0x01,
                    direction) #发送的数据
    uart3.write(temp)
    return

def receive_data_packet():
    '''
    @brief: 接收数据包
    '''
    global uart3
    bytesDatas = uart3.read()
    if(None == bytesDatas):
        return None
    return bytesDatas

def find_color(img):
    '''
    @biref  得到运输对象的颜色
    @retval 颜色的字符串列表
    '''
    colors = []
    flob = []
    if([] != img.find_blobs([colors_threshold['orange_threshold']], pixel_threshold = 250)):
        # colors.append(colors_threshold['orange_threshold'])
        colors.append('orange')

    if([] != img.find_blobs([colors_threshold['green_threshold']], pixel_threshold = 250)):
        # colors.append(colors_threshold['green_threshold'])
        colors.append('green')

    if([] != img.find_blobs([colors_threshold['blue_threshold']], pixel_threshold = 250)):
        # colors.append(colors_threshold['blue_threshold'])
        colors.append('blue')

    return colors

def find_crossing():
    global servoV, ServoH, led1
    servoH.servoAngle(105)
    servoV.servoAngle(135)
    if(True == right_angle.detectRightAngle()):
        led1.on()
        return car_move['stop']
    led1.off()
    return 0xff # car_move['go_forward']

def find_room(colors):
    '''
    @biref  查找房间的颜色并返回房间的方向
    @retval 小车的移动方向
    '''
    global servoH, servoV
    servoV.servoAngle(90)
    if(True == isAppearTarget(servoH, 105, 180, colors)):
        return car_move['turn_left']
    if(True == isAppearTarget(servoH, 30, 105, colors)):
        return car_move['turn_left']

    return car_move['go_forward']

def ScanH(servoH,angL,angR,thred):
    angLen = angR - angL
    gap = 11;
    subAng = angLen/gap;
    detectPosR = [0]*(gap+1);
    detectPosG = [0]*(gap+1);
    detectPosB = [0]*(gap+1);
    for i in range(0, gap + 1):
        servoH.servoAngle( angL + i*subAng)
        img = sensor.snapshot()
        ret = find_color(img)
        if( 'orange' in ret ):
            detectPosR[i] = detectPosR[i] + 1;
        if( 'green' in ret ):
            detectPosG[i] = detectPosG[i] + 1;
        if( 'blue' in ret ):
            detectPosB[i] = detectPosB[i] + 1;


    for i in range(0, gap + 1):
        j = gap-i
        servoH.servoAngle( angL + j*subAng)
        img = sensor.snapshot()
        ret = find_color(img)
        if( 'orange' in ret ):
            detectPosR[j] = detectPosR[j] + 1;
        if( 'green' in ret ):
            detectPosG[j] = detectPosG[j] + 1;
        if( 'blue' in ret ):
            detectPosB[j] = detectPosB[j] + 1;

    servoH.servoAngle(105)
    eleR = 0
    eleG = 0
    eleB = 0
    for i in range(0, gap + 1):
        eleR = eleR+detectPosR[i]*i*i
        eleG = eleG+detectPosG[i]*i*i
        eleB = eleB+detectPosB[i]*i*i

    if( sum(detectPosR)>thred ):
        eleR = angL + math.sqrt(eleR/(gap))*subAng
    else:
        eleR = -1

    if( sum(detectPosG)>thred ):
        eleG = angL + math.sqrt(eleG/(gap))*subAng
    else:
        eleG = -1

    if( sum(detectPosB)>thred ):
        eleB = angL + math.sqrt(eleB/(gap))*subAng
    else:
        eleB = -1

    return [eleR,eleG,eleB]

def isAppearTarget(servoH, angL, angR, colors):
    colorAngle = ScanH(servoH, angL, angR, 5)

    if(colorAngle[0] != -1):
        colors.remove('orange')
        return True
    if(colorAngle[1] != -1):
        colors.remove('green')
        return True
    if(colorAngle[2] != -1):
        colors.remove('blue')
        return True
    return False

def process(colors):
    global command
    data_packet = receive_data_packet()

    if(None != data_packet):
        for i in range(0,len(data_packet)):
            if((data_packet[i] == 0x55) and (data_packet[i+1] == 0x55)):
                command = data_packet[i+5]
                break

    if(0x00 == command):
        # 空操作,仅拍照
        img = sensor.snapshot()
        print('pass')

    elif(0x01 == command):
        send_data_packet(find_crossing())

    elif(0x03 == command):
        # sensor_init()
        send_data_packet(find_room(colors))
        # right_angle.sensor_rightAngle_init()
        print('room')


# Main function
if __name__ == "__main__":
    # Setup camera
    right_angle.rightAngle_init()
    uart3_init()
    #tim = Timer(4)              # create a timer object using timer 4  使用定时器4创建一个定时器对象
    #tim.init(freq=100)                # trigger at 100Hz 以100Hz触发
    #tim.callback(lambda t:process())
    led1 = LED(2)
    servoH = userServo(1, 'P7', 330)
    servoV = userServo(2, 'P8', 330)
    colors = None


    while(True):
        #find_crossing(sensor.snapshot())
        process(['orange', 'green', 'blue'])
        #send_data_packet(car_move['stop'])

